﻿# coding=utf-8
# 2023/4/27: 首个记录，基于旧2020/8/5版本
# 2023/5/5: 枚举基于Enum
# 2023/10/12: 补充部分英文注释
# 2024/3/19: 支持vehicle-sample-v5
# 2024/3/24: 支持object-vehicle-sample-v1@x通道
# 2025/4/19: 新增大灯位置坐标和相对于地面俯仰角字段

import bi_common as bi
from enum import Enum


class GearPosition(Enum):
    UNKNOWN = 0  # 未知
    PARK = 1  # P档
    REVERSE = 2  # R档
    NEUTRAL = 3  # N档
    DRIVE = 4  # D档


class TurnState(Enum):
    UNKNOWN = 0  # 未知
    NONE = 1  # 未打灯
    TURN_LEFT = 2  # 左转向灯
    TURN_RIGHT = 3  # 右转向灯
    EMERGENCY = 4  # 双闪


class Horn(Enum):
    UNKNOWN = 0  # 未知
    NONE = 1  # 不响
    ACTIVE = 2  # 响


class HeadLight(Enum):
    UNKNOWN = 0  # 未知
    NONE = 1  # 未激活
    NEAR = 2  # 近光
    FAR = 3  # 远光


class Wiper(Enum):
    UNKNOWN = 0  # 未知
    NONE = 1  # 未激活
    SINGLE = 2  # 单次
    SLOW1 = 3  # 慢 1
    SLOW2 = 4  # 慢 2
    SLOW3 = 5  # 慢 3
    SLOW4 = 6  # 慢 4
    SLOW5 = 7  # 慢 5
    MEDIUM = 8  # 中档
    FAST = 9  # 快档


class VehicleSample:
    def __init__(self):
        self.time = 0.0  # [s] Time offset in session / Session内的相对时间
        self.speed = 0.0  # [kph] 本车车速
        self.yaw_rate_valid = False
        self.yaw_rate = 0.0  # [deg/s] 横摆角速度
        self.curvature_valid = False
        self.curvature = 0.0  # [1/m] Turning left is positive, while the driving path is / 行驶曲率，左转为正，未来路径: y=(curvature/2)*(x^2)+(curvature)*(frontOverhang+wheelBase)*x
        self.kilometer_age = 0.0  # [km] 总里程数
        self.accel_x_valid = False
        self.accel_x = 0.0  # [m/s2] 本车纵向加速度
        self.accel_y_valid = False
        self.accel_y = 0.0  # [m/s2] 本车横向加速度
        self.jerk_x_valid = False
        self.jerk_x = 0.0  # [m/s3] 本车纵向急动度
        self.jerk_y_valid = False
        self.jerk_y = 0.0  # [m/s3] 本车横向急动度
        self.vehicle_width = 1.9  # [m] Constant / 车宽（常量）
        self.vehicle_length = 4.6  # [m] Constant / 车长（常量）
        self.vehicle_height = 1.5  # [m] Constant / 车高（常量）
        self.steer_angle_ratio = 15  # Constant / 传动比（常量）
        self.wheel_base = 2.8  # [m] Constant / 轴距（常量）
        self.rear_tread = 1.6  # [m] Constant / 轮距（常量）
        self.front_overhang = 0.9  # [m] Constant / 前悬（常量）
        self.max_curvature_valid = False
        self.max_curvature = 0.0  # [1/m] Max absolute curvature value (Constant) / 最大行驶曲率，不分左右（常量）
        self.max_acceleration_valid = False
        self.max_acceleration = 0.0  # [m/s2] Max absolute longitudinal acceleration value for forward driving (Constant) / 前向行驶最大加速度（常量）
        self.max_deceleration_valid = False
        self.max_deceleration = 0.0  # [m/s2] Max absolute longitudinal deceleration value for forward driving (Constant) / 前向行驶最大减速度（常量）
        self.max_accel_y_valid = False
        self.max_accel_y = 0.0  # [m/s2] Allowed max absolute lateral acceleration value (Constant) / 最大允许横向加速度，不分左右（常量）
        self.max_steer_angle_valid = False
        self.max_steer_angle = 0.0  # [deg] Constant / 最大方向盘转角（常量）
        self.max_steer_angle_rate_valid = False
        self.max_steer_angle_rate = 0.0  # [deg/s] Constant / 最大方向盘转角速度（常量）
        self.brake_dead_zone_valid = False
        self.brake_dead_zone = 0.0  # [%] Constant / 刹车踏板死区（常量）
        self.throttle_dead_zone_valid = False
        self.throttle_dead_zone = 0.0  # [%] Constant / 油门踏板死区（常量）
        self.fl_mass_valid = False
        self.fl_mass = 0.0  # [kg] Mass on front left wheel (Constant) / 左前轮上的质量（常量）
        self.fr_mass_valid = False
        self.fr_mass = 0.0  # [kg] Mass on front right wheel (Constant) / 右前轮上的质量（常量）
        self.rl_mass_valid = False
        self.rl_mass = 0.0  # [kg] Mass on rear left wheel (Constant) / 左后轮上的质量（常量）
        self.rr_mass_valid = False
        self.rr_mass = 0.0  # [kg] Mass on rear right wheel (Constant) / 右后轮上的质量（常量）
        self.front_cornering_stiffness_valid = False
        self.front_cornering_stiffness = 0.0  # [N/rad] Sum of front left and front right wheel's cornering stiffness (Constant) / 前轮侧偏刚度，左右轮之和（常量）
        self.rear_cornering_stiffness_valid = False
        self.rear_cornering_stiffness = 0.0  # [N/rad] Sum of rear left and rear right wheel's cornering stiffness (Constant) / 后轮侧偏刚度，左右轮之和（常量）
        self.steer_angle_valid = False
        self.steer_angle = 0.0  # [deg] 方向盘转角
        self.steer_angle_rate_valid = False
        self.steer_angle_rate = 0.0  # [deg/s] 方向盘转角速度
        self.steer_torque_valid = False
        self.steer_torque = 0.0  # [Nm] 方向盘力矩
        self.brake_valid = False
        self.brake = 0.0  # [%] 刹车位置
        self.throttle_valid = False
        self.throttle = 0.0  # [%] 油门位置
        self.gear = GearPosition.UNKNOWN  # 档位
        self.turn = TurnState.UNKNOWN  # 转向灯
        self.horn = Horn.UNKNOWN  # 喇叭
        self.head_light = HeadLight.UNKNOWN  # 大灯
        self.wiper = Wiper.UNKNOWN  # 雨刮
        self.fl_speed_valid = False
        self.fl_speed = 0.0  # [kph] Speed of front left wheel / 左前轮轮速
        self.fr_speed_valid = False
        self.fr_speed = 0.0  # [kph] Speed of front right wheel / 右前轮轮速
        self.rl_speed_valid = False
        self.rl_speed = 0.0  # [kph] Speed of rear left wheel / 左后轮轮速
        self.rr_speed_valid = False
        self.rr_speed = 0.0  # [kph] Speed of rear right wheel / 右后轮轮速
        self.fl_angle_valid = False
        self.fl_angle = 0.0  # [deg] Direction of front left wheel / 左前轮朝向角
        self.fr_angle_valid = False
        self.fr_angle = 0.0  # [deg] Direction of front right wheel / 右前轮朝向角
        self.engine_speed_valid = False
        self.engine_speed = 0.0  # [rpm] 引擎转速
        self.engine_torque_valid = False
        self.engine_torque = 0.0  # [Nm] 引擎扭矩
        self.front_end_lateral_speed_valid = False
        self.front_end_lateral_speed = 0.0  # [m/s] 前保中心横向速度
        self.rear_end_lateral_speed_valid = False
        self.rear_end_lateral_speed = 0.0  # [m/s] 后保中心横向速度
        self.fl_light_pos_x_valid = False
        self.fl_light_pos_x = 0.0  # [m] X Coordinate of front left light (the same as front right light) / 左前灯X坐标(即右前灯X坐标)
        self.fl_light_pos_y_valid = False
        self.fl_light_pos_y = 0.0  # [m] Y Coordinate of front left light (negative value of front right light) / 左前灯Y坐标(右前灯Y坐标为其负值)
        self.fl_light_pos_z_valid = False
        self.fl_light_pos_z = 0.0  # [m] Z Coordinate of front left light (the same as front right light) / 左前灯Z坐标(即右前灯Z坐标)
        self.rl_light_pos_x_valid = False
        self.rl_light_pos_x = 0.0  # [m] X Coordinate of rear left light (the same as rear right light) / 左后灯X坐标(即右后灯X坐标)
        self.rl_light_pos_y_valid = False
        self.rl_light_pos_y = 0.0  # [m] Y Coordinate of rear left light (negative value of rear right light) / 左后灯Y坐标(右后灯Y坐标为其负值)
        self.rl_light_pos_z_valid = False
        self.rl_light_pos_z = 0.0  # [m] Z Coordinate of rear left light (the same as rear right light) / 左后灯Z坐标(即右后灯Z坐标)
        self.pitch_to_ground_valid = False
        self.pitch_to_ground = 0.0  # [deg] Vehicle's pitch angle to ground / 车辆相对于地面的俯仰角

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self):
        output = bi.agency.create_general_sample()
        output.protocol = "vehicle-sample-v4"
        output.time = self.time
        output.significant = 40
        output.values = []
        output.values.append(0)
        output.values.append(0)
        output.values.append(self.speed)
        output.values.append(self.yaw_rate if self.yaw_rate_valid else None)
        output.values.append(self.curvature if self.curvature_valid else None)
        output.values.append(self.accel_x if self.accel_x_valid else None)
        output.values.append(self.accel_y if self.accel_y_valid else None)
        output.values.append(self.vehicle_width)
        output.values.append(self.vehicle_length)
        output.values.append(self.vehicle_height)
        output.values.append(self.steer_angle_ratio)
        output.values.append(self.wheel_base)
        output.values.append(self.rear_tread)
        output.values.append(self.front_overhang)
        output.values.append(self.steer_angle if self.steer_angle_valid else None)
        output.values.append(self.steer_angle_rate if self.steer_angle_rate_valid else None)
        output.values.append(self.brake if self.brake_valid else None)
        output.values.append(self.throttle if self.throttle_valid else None)
        output.values.append(self.gear.value)
        output.values.append(self.turn.value)
        output.values.append(self.fl_speed if self.fl_speed_valid else None)
        output.values.append(self.fr_speed if self.fr_speed_valid else None)
        output.values.append(self.rl_speed if self.rl_speed_valid else None)
        output.values.append(self.rr_speed if self.rr_speed_valid else None)
        output.values.append(self.engine_speed if self.engine_speed_valid else None)
        output.values.append(self.engine_torque if self.engine_torque_valid else None)
        output.values.append(self.horn.value)
        output.values.append(self.head_light.value)
        output.values.append(self.wiper.value)
        output.values.append(self.kilometer_age)
        output.values.append(self.front_end_lateral_speed if self.front_end_lateral_speed_valid else None)
        output.values.append(self.rear_end_lateral_speed if self.rear_end_lateral_speed_valid else None)
        output.values.append(self.steer_torque if self.steer_torque_valid else None)
        output.values.append(self.jerk_x if self.jerk_x_valid else None)
        output.values.append(self.jerk_y if self.jerk_y_valid else None)
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        return output

    def _to_general_sample_v5(self, is_subject, object_channel):
        output = bi.agency.create_general_sample()
        output.protocol = "vehicle-sample-v5" if is_subject else ("object-vehicle-sample-v1@" + object_channel)
        output.time = self.time
        output.significant = 80
        output.values = []
        output.values.append(0)
        output.values.append(0)
        output.values.append(self.speed)
        output.values.append(self.yaw_rate if self.yaw_rate_valid else None)
        output.values.append(self.curvature if self.curvature_valid else None)
        output.values.append(self.accel_x if self.accel_x_valid else None)
        output.values.append(self.accel_y if self.accel_y_valid else None)
        output.values.append(self.vehicle_width)
        output.values.append(self.vehicle_length)
        output.values.append(self.vehicle_height)
        output.values.append(self.steer_angle_ratio)
        output.values.append(self.wheel_base)
        output.values.append(self.rear_tread)
        output.values.append(self.front_overhang)
        output.values.append(self.steer_angle if self.steer_angle_valid else None)
        output.values.append(self.steer_angle_rate if self.steer_angle_rate_valid else None)
        output.values.append(self.brake if self.brake_valid else None)
        output.values.append(self.throttle if self.throttle_valid else None)
        output.values.append(self.gear.value)
        output.values.append(self.turn.value)
        output.values.append(self.fl_speed if self.fl_speed_valid else None)
        output.values.append(self.fr_speed if self.fr_speed_valid else None)
        output.values.append(self.rl_speed if self.rl_speed_valid else None)
        output.values.append(self.rr_speed if self.rr_speed_valid else None)
        output.values.append(self.engine_speed if self.engine_speed_valid else None)
        output.values.append(self.engine_torque if self.engine_torque_valid else None)
        output.values.append(self.horn.value)
        output.values.append(self.head_light.value)
        output.values.append(self.wiper.value)
        output.values.append(self.kilometer_age)
        output.values.append(self.front_end_lateral_speed if self.front_end_lateral_speed_valid else None)
        output.values.append(self.rear_end_lateral_speed if self.rear_end_lateral_speed_valid else None)
        output.values.append(self.steer_torque if self.steer_torque_valid else None)
        output.values.append(self.jerk_x if self.jerk_x_valid else None)
        output.values.append(self.jerk_y if self.jerk_y_valid else None)
        output.values.append(self.max_curvature if self.max_curvature_valid else None)
        output.values.append(self.max_acceleration if self.max_acceleration_valid else None)
        output.values.append(self.max_deceleration if self.max_deceleration_valid else None)
        output.values.append(self.max_accel_y if self.max_accel_y_valid else None)
        output.values.append(self.max_steer_angle if self.max_steer_angle_valid else None)
        output.values.append(self.max_steer_angle_rate if self.max_steer_angle_rate_valid else None)
        output.values.append(self.brake_dead_zone if self.brake_dead_zone_valid else None)
        output.values.append(self.throttle_dead_zone if self.throttle_dead_zone_valid else None)
        output.values.append(self.fl_mass if self.fl_mass_valid else None)
        output.values.append(self.fr_mass if self.fr_mass_valid else None)
        output.values.append(self.rl_mass if self.rl_mass_valid else None)
        output.values.append(self.rr_mass if self.rr_mass_valid else None)
        output.values.append(self.fl_angle if self.fl_angle_valid else None)
        output.values.append(self.fr_angle if self.fr_angle_valid else None)
        output.values.append(self.front_cornering_stiffness if self.front_cornering_stiffness_valid else None)
        output.values.append(self.rear_cornering_stiffness if self.rear_cornering_stiffness_valid else None)
        output.values.append(self.fl_light_pos_x if self.fl_light_pos_x_valid else None)
        output.values.append(self.fl_light_pos_y if self.fl_light_pos_y_valid else None)
        output.values.append(self.fl_light_pos_z if self.fl_light_pos_z_valid else None)
        output.values.append(self.rl_light_pos_x if self.rl_light_pos_x_valid else None)
        output.values.append(self.rl_light_pos_y if self.rl_light_pos_y_valid else None)
        output.values.append(self.rl_light_pos_z if self.rl_light_pos_z_valid else None)
        output.values.append(self.pitch_to_ground if self.pitch_to_ground_valid else None)
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        return output
    
    # Convert subject vehicle info to general sample / 将主车车辆信息转为通用样本，用于样本输出
    def to_general_sample_v5_as_subject(self):
        return self._to_general_sample_v5(True, None)
    
    # Convert object vehicle info to general sample / 将目标物车辆信息转为通用样本，用于样本输出
    def to_general_sample_v5_as_object(self, channel):
        return self._to_general_sample_v5(False, channel)


def _conv_vehicle_sample_v3(gs):
    values_count = len(gs.values)
    if values_count < 27:
        return None
    trajectory_size = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count != 27 and values_count != 27 + 2 * trajectory_size:
        return None
    output = VehicleSample()
    output.time = gs.time
    output.speed = float(gs.values[1]) if gs.values[1] is not None else 0.0
    if gs.values[2] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[2])
    if gs.values[3] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[3])
    if gs.values[4] is not None:
        output.accel_x_valid = True
        output.accel_x = float(gs.values[4])
    if gs.values[5] is not None:
        output.accel_y_valid = True
        output.accel_y = float(gs.values[5])
    output.vehicle_width = float(gs.values[6]) if gs.values[6] is not None else 1.9
    output.vehicle_length = float(gs.values[7]) if gs.values[7] is not None else 4.6
    output.steer_angle_ratio = float(gs.values[8]) if gs.values[8] is not None else 15.0
    output.wheel_base = float(gs.values[9]) if gs.values[9] is not None else 2.8
    output.rear_tread = float(gs.values[10]) if gs.values[10] is not None else 1.6
    output.front_overhang = float(gs.values[11]) if gs.values[11] is not None else 0.9
    if gs.values[12] is not None:
        output.steer_angle_valid = True
        output.steer_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.brake_valid = True
        output.brake = float(gs.values[13])
    if gs.values[14] is not None:
        output.throttle_valid = True
        output.throttle = float(gs.values[14])
    output.gear = GearPosition(int(gs.values[15])) if gs.values[15] is not None else GearPosition.UNKNOWN
    output.turn = TurnState(int(gs.values[16])) if gs.values[16] is not None else TurnState.UNKNOWN
    if gs.values[17] is not None:
        output.fl_speed_valid = True
        output.fl_speed = float(gs.values[17])
    if gs.values[18] is not None:
        output.fr_speed_valid = True
        output.fr_speed = float(gs.values[18])
    if gs.values[19] is not None:
        output.rl_speed_valid = True
        output.rl_speed = float(gs.values[19])
    if gs.values[20] is not None:
        output.rr_speed_valid = True
        output.rr_speed = float(gs.values[20])
    if gs.values[21] is not None:
        output.engine_speed_valid = True
        output.engine_speed = float(gs.values[21])
    if gs.values[22] is not None:
        output.engine_torque_valid = True
        output.engine_torque = float(gs.values[22])
    output.horn = Horn(int(gs.values[23])) if gs.values[23] is not None else Horn.UNKNOWN
    output.head_light = HeadLight(int(gs.values[24])) if gs.values[24] is not None else HeadLight.UNKNOWN
    output.wiper = Wiper(int(gs.values[25])) if gs.values[25] is not None else Wiper.UNKNOWN
    output.kilometer_age = float(gs.values[26]) if gs.values[26] is not None else 0.0
    return output


def _conv_vehicle_sample_v4(gs):
    values_count = len(gs.values)
    if values_count < 40:
        return None
    contour_size = int(gs.values[0]) if gs.values[0] is not None else 0
    trajectory_size = int(gs.values[1]) if gs.values[1] is not None else 0
    if values_count != 40 + 2 * contour_size and values_count != 40 + 2 * (contour_size + trajectory_size):
        return None
    output = VehicleSample()
    output.time = gs.time
    output.speed = float(gs.values[2]) if gs.values[2] is not None else 0.0
    if gs.values[3] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[3])
    if gs.values[4] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[4])
    if gs.values[5] is not None:
        output.accel_x_valid = True
        output.accel_x = float(gs.values[5])
    if gs.values[6] is not None:
        output.accel_y_valid = True
        output.accel_y = float(gs.values[6])
    output.vehicle_width = float(gs.values[7]) if gs.values[7] is not None else 1.9
    output.vehicle_length = float(gs.values[8]) if gs.values[8] is not None else 4.6
    output.vehicle_height = float(gs.values[9]) if gs.values[9] is not None else 1.5
    output.steer_angle_ratio = float(gs.values[10]) if gs.values[10] is not None else 15.0
    output.wheel_base = float(gs.values[11]) if gs.values[11] is not None else 2.8
    output.rear_tread = float(gs.values[12]) if gs.values[12] is not None else 1.6
    output.front_overhang = float(gs.values[13]) if gs.values[13] is not None else 0.9
    if gs.values[14] is not None:
        output.steer_angle_valid = True
        output.steer_angle = float(gs.values[14])
    if gs.values[15] is not None:
        output.steer_angle_rate_valid = True
        output.steer_angle_rate = float(gs.values[15])
    if gs.values[16] is not None:
        output.brake_valid = True
        output.brake = float(gs.values[16])
    if gs.values[17] is not None:
        output.throttle_valid = True
        output.throttle = float(gs.values[17])
    output.gear = GearPosition(int(gs.values[18])) if gs.values[18] is not None else GearPosition.UNKNOWN
    output.turn = TurnState(int(gs.values[19])) if gs.values[19] is not None else TurnState.UNKNOWN
    if gs.values[20] is not None:
        output.fl_speed_valid = True
        output.fl_speed = float(gs.values[20])
    if gs.values[21] is not None:
        output.fr_speed_valid = True
        output.fr_speed = float(gs.values[21])
    if gs.values[22] is not None:
        output.rl_speed_valid = True
        output.rl_speed = float(gs.values[22])
    if gs.values[23] is not None:
        output.rr_speed_valid = True
        output.rr_speed = float(gs.values[23])
    if gs.values[24] is not None:
        output.engine_speed_valid = True
        output.engine_speed = float(gs.values[24])
    if gs.values[25] is not None:
        output.engine_torque_valid = True
        output.engine_torque = float(gs.values[25])
    output.horn = Horn(int(gs.values[26])) if gs.values[26] is not None else Horn.UNKNOWN
    output.head_light = HeadLight(int(gs.values[27])) if gs.values[27] is not None else HeadLight.UNKNOWN
    output.wiper = Wiper(int(gs.values[28])) if gs.values[28] is not None else Wiper.UNKNOWN
    output.kilometer_age = float(gs.values[29]) if gs.values[29] is not None else 0.0
    if gs.values[30] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[30])
    if gs.values[31] is not None:
        output.rear_end_lateral_speed_valid = True
        output.rear_end_lateral_speed = float(gs.values[31])
    if gs.values[32] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[32])
    if gs.values[33] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[33])
    if gs.values[34] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[34])
    return output


def _conv_vehicle_sample_v5(gs):
    values_count = len(gs.values)
    if values_count < 80:
        return None
    contour_size = int(gs.values[0]) if gs.values[0] is not None else 0
    trajectory_size = int(gs.values[1]) if gs.values[1] is not None else 0
    if values_count != 80 + 2 * contour_size and values_count != 80 + 2 * (contour_size + trajectory_size):
        return None
    output = VehicleSample()
    output.time = gs.time
    output.speed = float(gs.values[2]) if gs.values[2] is not None else 0.0
    if gs.values[3] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[3])
    if gs.values[4] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[4])
    if gs.values[5] is not None:
        output.accel_x_valid = True
        output.accel_x = float(gs.values[5])
    if gs.values[6] is not None:
        output.accel_y_valid = True
        output.accel_y = float(gs.values[6])
    output.vehicle_width = float(gs.values[7]) if gs.values[7] is not None else 1.9
    output.vehicle_length = float(gs.values[8]) if gs.values[8] is not None else 4.6
    output.vehicle_height = float(gs.values[9]) if gs.values[9] is not None else 1.5
    output.steer_angle_ratio = float(gs.values[10]) if gs.values[10] is not None else 15.0
    output.wheel_base = float(gs.values[11]) if gs.values[11] is not None else 2.8
    output.rear_tread = float(gs.values[12]) if gs.values[12] is not None else 1.6
    output.front_overhang = float(gs.values[13]) if gs.values[13] is not None else 0.9
    if gs.values[14] is not None:
        output.steer_angle_valid = True
        output.steer_angle = float(gs.values[14])
    if gs.values[15] is not None:
        output.steer_angle_rate_valid = True
        output.steer_angle_rate = float(gs.values[15])
    if gs.values[16] is not None:
        output.brake_valid = True
        output.brake = float(gs.values[16])
    if gs.values[17] is not None:
        output.throttle_valid = True
        output.throttle = float(gs.values[17])
    output.gear = GearPosition(int(gs.values[18])) if gs.values[18] is not None else GearPosition.UNKNOWN
    output.turn = TurnState(int(gs.values[19])) if gs.values[19] is not None else TurnState.UNKNOWN
    if gs.values[20] is not None:
        output.fl_speed_valid = True
        output.fl_speed = float(gs.values[20])
    if gs.values[21] is not None:
        output.fr_speed_valid = True
        output.fr_speed = float(gs.values[21])
    if gs.values[22] is not None:
        output.rl_speed_valid = True
        output.rl_speed = float(gs.values[22])
    if gs.values[23] is not None:
        output.rr_speed_valid = True
        output.rr_speed = float(gs.values[23])
    if gs.values[24] is not None:
        output.engine_speed_valid = True
        output.engine_speed = float(gs.values[24])
    if gs.values[25] is not None:
        output.engine_torque_valid = True
        output.engine_torque = float(gs.values[25])
    output.horn = Horn(int(gs.values[26])) if gs.values[26] is not None else Horn.UNKNOWN
    output.head_light = HeadLight(int(gs.values[27])) if gs.values[27] is not None else HeadLight.UNKNOWN
    output.wiper = Wiper(int(gs.values[28])) if gs.values[28] is not None else Wiper.UNKNOWN
    output.kilometer_age = float(gs.values[29]) if gs.values[29] is not None else 0.0
    if gs.values[30] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[30])
    if gs.values[31] is not None:
        output.rear_end_lateral_speed_valid = True
        output.rear_end_lateral_speed = float(gs.values[31])
    if gs.values[32] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[32])
    if gs.values[33] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[33])
    if gs.values[34] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[34])
    if gs.values[35] is not None:
        output.max_curvature_valid = True
        output.max_curvature = float(gs.values[35])
    if gs.values[36] is not None:
        output.max_acceleration_valid = True
        output.max_acceleration = float(gs.values[36])
    if gs.values[37] is not None:
        output.max_deceleration_valid = True
        output.max_deceleration = float(gs.values[37])
    if gs.values[38] is not None:
        output.max_accel_y_valid = True
        output.max_accel_y = float(gs.values[38])
    if gs.values[39] is not None:
        output.max_steer_angle_valid = True
        output.max_steer_angle = float(gs.values[39])
    if gs.values[40] is not None:
        output.max_steer_angle_rate_valid = True
        output.max_steer_angle_rate = float(gs.values[40])
    if gs.values[41] is not None:
        output.brake_dead_zone_valid = True
        output.brake_dead_zone = float(gs.values[41])
    if gs.values[42] is not None:
        output.throttle_dead_zone_valid = True
        output.throttle_dead_zone = float(gs.values[42])
    if gs.values[43] is not None:
        output.fl_mass_valid = True
        output.fl_mass = float(gs.values[43])
    if gs.values[44] is not None:
        output.fr_mass_valid = True
        output.fr_mass = float(gs.values[44])
    if gs.values[45] is not None:
        output.rl_mass_valid = True
        output.rl_mass = float(gs.values[45])
    if gs.values[46] is not None:
        output.rr_mass_valid = True
        output.rr_mass = float(gs.values[46])
    if gs.values[47] is not None:
        output.fl_angle_valid = True
        output.fl_angle = float(gs.values[47])
    if gs.values[48] is not None:
        output.fr_angle_valid = True
        output.fr_angle = float(gs.values[48])
    if gs.values[49] is not None:
        output.front_cornering_stiffness_valid = True
        output.front_cornering_stiffness = float(gs.values[49])
    if gs.values[50] is not None:
        output.rear_cornering_stiffness_valid = True
        output.rear_cornering_stiffness = float(gs.values[50])
    if gs.values[51] is not None:
        output.fl_light_pos_x_valid = True
        output.fl_light_pos_x = float(gs.values[51])
    if gs.values[52] is not None:
        output.fl_light_pos_y_valid = True
        output.fl_light_pos_y = float(gs.values[52])
    if gs.values[53] is not None:
        output.fl_light_pos_z_valid = True
        output.fl_light_pos_z = float(gs.values[53])
    if gs.values[54] is not None:
        output.rl_light_pos_x_valid = True
        output.rl_light_pos_x = float(gs.values[54])
    if gs.values[55] is not None:
        output.rl_light_pos_y_valid = True
        output.rl_light_pos_y = float(gs.values[55])
    if gs.values[56] is not None:
        output.rl_light_pos_z_valid = True
        output.rl_light_pos_z = float(gs.values[56])
    if gs.values[57] is not None:
        output.pitch_to_ground_valid = True
        output.pitch_to_ground = float(gs.values[57])
    return output


def _interpolate_vehicle_sample(vs1, w1, vs2, w2):
    output = VehicleSample()
    output.time = bi.time
    output.speed = vs1.speed * w1 + vs2.speed * w2
    output.kilometer_age = vs1.kilometer_age * w1 + vs2.kilometer_age * w2
    if vs1.yaw_rate_valid and vs2.yaw_rate_valid:
        output.yaw_rate_valid = True
        output.yaw_rate = vs1.yaw_rate * w1 + vs2.yaw_rate * w2
    if vs1.curvature_valid and vs2.curvature_valid:
        output.curvature_valid = True
        output.curvature = vs1.curvature * w1 + vs2.curvature * w2
    if vs1.accel_x_valid and vs2.accel_x_valid:
        output.accel_x_valid = True
        output.accel_x = vs1.accel_x * w1 + vs2.accel_x * w2
    if vs1.accel_y_valid and vs2.accel_y_valid:
        output.accel_y_valid = True
        output.accel_y = vs1.accel_y * w1 + vs2.accel_y * w2
    if vs1.jerk_x_valid and vs2.jerk_x_valid:
        output.jerk_x_valid = True
        output.jerk_x = vs1.jerk_x * w1 + vs2.jerk_x * w2
    if vs1.jerk_y_valid and vs2.jerk_y_valid:
        output.jerk_y_valid = True
        output.jerk_y = vs1.jerk_y * w1 + vs2.jerk_y * w2
    output.vehicle_width = vs1.vehicle_width
    output.vehicle_length = vs1.vehicle_length
    output.vehicle_height = vs1.vehicle_height
    output.steer_angle_ratio = vs1.steer_angle_ratio
    output.wheel_base = vs1.wheel_base
    output.rear_tread = vs1.rear_tread
    output.front_overhang = vs1.front_overhang
    if vs1.steer_angle_valid and vs2.steer_angle_valid:
        output.steer_angle_valid = True
        output.steer_angle = vs1.steer_angle * w1 + vs2.steer_angle * w2
    if vs1.steer_angle_rate_valid and vs2.steer_angle_rate_valid:
        output.steer_angle_rate_valid = True
        output.steer_angle_rate = vs1.steer_angle_rate * w1 + vs2.steer_angle_rate * w2
    if vs1.steer_torque_valid and vs2.steer_torque_valid:
        output.steer_torque_valid = True
        output.steer_torque = vs1.steer_torque * w1 + vs2.steer_torque * w2
    if vs1.brake_valid and vs2.brake_valid:
        output.brake_valid = True
        output.brake = vs1.brake * w1 + vs2.brake * w2
    if vs1.throttle_valid and vs2.throttle_valid:
        output.throttle_valid = True
        output.throttle = vs1.throttle * w1 + vs2.throttle * w2
    output.gear = vs1.gear if w1 > w2 else vs2.gear
    output.turn = vs1.turn if w1 > w2 else vs2.turn
    output.horn = vs1.horn if w1 > w2 else vs2.horn
    output.head_light = vs1.head_light if w1 > w2 else vs2.head_light
    output.wiper = vs1.wiper if w1 > w2 else vs2.wiper
    if vs1.fl_speed_valid and vs2.fl_speed_valid:
        output.fl_speed_valid = True
        output.fl_speed = vs1.fl_speed * w1 + vs2.fl_speed * w2
    if vs1.fr_speed_valid and vs2.fr_speed_valid:
        output.fr_speed_valid = True
        output.fr_speed = vs1.fr_speed * w1 + vs2.fr_speed * w2
    if vs1.rl_speed_valid and vs2.rl_speed_valid:
        output.rl_speed_valid = True
        output.rl_speed = vs1.rl_speed * w1 + vs2.rl_speed * w2
    if vs1.rr_speed_valid and vs2.rr_speed_valid:
        output.rr_speed_valid = True
        output.rr_speed = vs1.rr_speed * w1 + vs2.rr_speed * w2
    if vs1.engine_speed_valid and vs2.engine_speed_valid:
        output.engine_speed_valid = True
        output.engine_speed = vs1.engine_speed * w1 + vs2.engine_speed * w2
    if vs1.engine_torque_valid and vs2.engine_torque_valid:
        output.engine_torque_valid = True
        output.engine_torque = vs1.engine_torque * w1 + vs2.engine_torque * w2
    if vs1.front_end_lateral_speed_valid and vs2.front_end_lateral_speed_valid:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = vs1.front_end_lateral_speed * w1 + vs2.front_end_lateral_speed * w2
    if vs1.rear_end_lateral_speed_valid and vs2.rear_end_lateral_speed_valid:
        output.rear_end_lateral_speed_valid = True
        output.rear_end_lateral_speed = vs1.rear_end_lateral_speed * w1 + vs2.rear_end_lateral_speed * w2
    output.max_curvature_valid = vs1.max_curvature_valid
    output.max_curvature = vs1.max_curvature
    output.max_acceleration_valid = vs1.max_acceleration_valid
    output.max_acceleration = vs1.max_acceleration
    output.max_deceleration_valid = vs1.max_deceleration_valid
    output.max_deceleration = vs1.max_deceleration
    output.max_accel_y_valid = vs1.max_accel_y_valid
    output.max_accel_y = vs1.max_accel_y
    output.max_steer_angle_valid = vs1.max_steer_angle_valid
    output.max_steer_angle = vs1.max_steer_angle
    output.max_steer_angle_rate_valid = vs1.max_steer_angle_rate_valid
    output.max_steer_angle_rate = vs1.max_steer_angle_rate
    output.brake_dead_zone_valid = vs1.brake_dead_zone_valid
    output.brake_dead_zone = vs1.brake_dead_zone
    output.throttle_dead_zone_valid = vs1.throttle_dead_zone_valid
    output.throttle_dead_zone = vs1.throttle_dead_zone
    output.fl_mass_valid = vs1.fl_mass_valid
    output.fl_mass = vs1.fl_mass
    output.fr_mass_valid = vs1.fr_mass_valid
    output.fr_mass = vs1.fr_mass
    output.rl_mass_valid = vs1.rl_mass_valid
    output.rl_mass = vs1.rl_mass
    output.rr_mass_valid = vs1.rr_mass_valid
    output.rr_mass = vs1.rr_mass
    if vs1.fl_angle_valid and vs2.fl_angle_valid:
        output.fl_angle_valid = True
        output.fl_angle = vs1.fl_angle * w1 + vs2.fl_angle * w2
    if vs1.fr_angle_valid and vs2.fr_angle_valid:
        output.fr_angle_valid = True
        output.fr_angle = vs1.fr_angle * w1 + vs2.fr_angle * w2
    output.front_cornering_stiffness_valid = vs1.front_cornering_stiffness_valid
    output.front_cornering_stiffness = vs1.front_cornering_stiffness
    output.rear_cornering_stiffness_valid = vs1.rear_cornering_stiffness_valid
    output.rear_cornering_stiffness = vs1.rear_cornering_stiffness
    output.fl_light_pos_x_valid = vs1.fl_light_pos_x_valid if w1 > w2 else vs2.fl_light_pos_x_valid
    output.fl_light_pos_x = vs1.fl_light_pos_x if w1 > w2 else vs2.fl_light_pos_x
    output.fl_light_pos_y_valid = vs1.fl_light_pos_y_valid if w1 > w2 else vs2.fl_light_pos_y_valid
    output.fl_light_pos_y = vs1.fl_light_pos_y if w1 > w2 else vs2.fl_light_pos_y
    output.fl_light_pos_z_valid = vs1.fl_light_pos_z_valid if w1 > w2 else vs2.fl_light_pos_z_valid
    output.fl_light_pos_z = vs1.fl_light_pos_z if w1 > w2 else vs2.fl_light_pos_z
    output.rl_light_pos_x_valid = vs1.rl_light_pos_x_valid if w1 > w2 else vs2.rl_light_pos_x_valid
    output.rl_light_pos_x = vs1.rl_light_pos_x if w1 > w2 else vs2.rl_light_pos_x
    output.rl_light_pos_y_valid = vs1.rl_light_pos_y_valid if w1 > w2 else vs2.rl_light_pos_y_valid
    output.rl_light_pos_y = vs1.rl_light_pos_y if w1 > w2 else vs2.rl_light_pos_y
    output.rl_light_pos_z_valid = vs1.rl_light_pos_z_valid if w1 > w2 else vs2.rl_light_pos_z_valid
    output.rl_light_pos_z = vs1.rl_light_pos_z if w1 > w2 else vs2.rl_light_pos_z
    if vs1.pitch_to_ground_valid and vs2.pitch_to_ground_valid:
        output.pitch_to_ground_valid = True
        output.pitch_to_ground = vs1.pitch_to_ground * w1 + vs2.pitch_to_ground * w2
    return output


# Query subject's VehicleSample for input / 获取主车VehicleSample，用于样本输入
def get_subject_vehicle_sample():
    vs1 = None
    vs2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v3 = 'vehicle-sample-v3'
    protocol_id_v4 = 'vehicle-sample-v4'
    protocol_id_v5 = 'vehicle-sample-v5'
    if protocol_id_v5 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v5]
        vs1 = _conv_vehicle_sample_v5(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_vehicle_sample_v5(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        vs1 = _conv_vehicle_sample_v4(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_vehicle_sample_v4(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v3 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v3]
        vs1 = _conv_vehicle_sample_v3(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_vehicle_sample_v3(pair.sample2)
        w2 = pair.weight2
    if vs1 is not None and vs2 is not None:
        return _interpolate_vehicle_sample(vs1, w1, vs2, w2)
    return None


# Query object's VehicleSample for input / 获取目标物VehicleSample，用于样本输入
def get_object_vehicle_sample(channel):
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v1 = 'object-vehicle-sample-v1@' + str(channel)
    if protocol_id_v1 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v1]
        s1 = _conv_vehicle_sample_v5(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_vehicle_sample_v5(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return _interpolate_vehicle_sample(s1, w1, s2, w2)
    return None


# Query subject's VehicleSample for input / 获取主车VehicleSample，用于样本输入
def get_vehicle_sample():
    return get_subject_vehicle_sample()
